/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file rssi_ukf.cxx
	\brief class implemenation for rssi_ukf

                    \author Jack Elston
             $Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>

#include "rssiUKF.h"

  /*<-------Defines--------->*/
  /*<------End Defines------>*/

  /*<---Global Variables---->*/
    extern bool verbose;
    extern bool logging;
    extern String logDir;
  /*<-End Global Variables-->*/

RSSIUKF::RSSIUKF(String name) : ThreadedPipe(name)
{
}

bool RSSIUKF::init(ShmStruct * shmStructPtr)
{
	ThreadedPipe::init(shmStructPtr);

	Client * me = shmPtr->clients.getClient(0);
  subscribe(CMD_RSSI_TELEM, me, true, INBOARD_TX);

	/*--------- log ----------*/
	if(logging) {
    log_name = generteNumberedFile( logDir, "rssi_log_");

    // Write the header
    logfile=fopen(log_name.c_str(), "a");
    fprintf(logfile,"%s\n", "%mode(1) time(1) X_s(4) signal(1) X(4) P(16) u(1)");
    if(logfile != NULL) {
      fflush(logfile);
      fclose(logfile);
    }
	}
	return true;
}

void RSSIUKF::update() 
{
  int val, maxFD;
	fd_set readFDs, writeFDs;                   // fd sets
	struct timeval timeout;

  FD_ZERO(&readFDs);	                    		// zero read fd set
  FD_ZERO(&writeFDs);	                    		// zero write fd set
  FD_SET(fifo_FDs[INBOARD_RX],&readFDs);			// add fifo
  FD_SET(fifo_FDs[OUTBOARD_RX],&readFDs);			// add fifo
  FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);			// add fifo
  FD_SET(fifo_FDs[OUTBOARD_TX],&writeFDs);		// add fifo

  fifo_FDs[INBOARD_RX] > fifo_FDs[OUTBOARD_RX] ? maxFD = fifo_FDs[INBOARD_RX] : maxFD = fifo_FDs[OUTBOARD_RX];
  if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];
  if(fifo_FDs[OUTBOARD_TX] > maxFD) maxFD = fifo_FDs[OUTBOARD_TX];

  timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

  Client * me = shmPtr->clients.getClient(0);

  if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
    if( val != 0 ) {
      if (errno != EINTR) {
        perror("RSSIUKF::update() select");
        execRate = -1;
      } else
        cout << getName() << "::update() - select interrupted" << endl;
    }
  }
  else {
    if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs))
    {
      int n = 0;
      n=readFifo(INBOARD_RX);
      if(n>0) {
        uint16_t length = CMD_SIZE(fifo_msg.data);
        if( length == fifo_msg.size) {

          Command cmd((unsigned char *)fifo_msg.data);
          if( !cmd.isValid() )
            cout << getName() << "::update() - error, invalid command" << endl << cmd ;
          else {
            parseInternalData(cmd);
					}
				}
      }
    }
  }
}

void RSSIUKF::parseInternalData(const Command &cmd)
{
 	Client * a_client = shmPtr->clients.getClient(fifo_msg.src);
	Client * me = shmPtr->clients.getClient(0);

	if(a_client == NULL) return;

	uint8_t * dataPtr = CMD_DATAPTR(fifo_msg.data);

  switch(cmd.cmd() ) {
		case CMD_RSSI_TELEM: { // Got a new measurement packet
		  memcpy(&meas, dataPtr, sizeof(RSSIStruct_t));
      update_ukf();
      break;
    }
  }
}

void RSSIUKF::update_ukf()
{
  // Convert data to UKF format

  // Run the filter

  // Write to the log file
  if(logging){
    logfile=fopen(log_name.c_str(), "a");
    if(logfile != NULL) {
      update_log_str();
      fprintf(logfile,"%s\n", logStr);
      fflush(logfile);
      fclose(logfile);
    }
  }


}
void RSSIUKF::update_log_str()
{
	int n=0;
	currentStamp.stamp();
	n += sprintf(logStr + n,"%.1f ",currentStamp.time());
	n += sprintf(logStr + n,"%f %f %f %f ",meas.gps.lat, meas.gps.lon, meas.gps.alt, meas.rssi);
}

